/*
 * Copyright (c) 2025 STMROS Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include "stmconn/stmconn_interface.hpp"
#include "rclcpp/rclcpp.hpp"
#include "stmros_msgs/msg/stm_heartbeat.hpp"
#include "stmros_msgs/srv/stm_command.hpp"
// 前向声明以避免循环依赖
namespace stmros {
namespace plugin {
class Plugin;
using PluginPtr = std::shared_ptr<Plugin>;
using HandlerCb = std::function<void(const std::vector<uint8_t>& payload)>;
} // namespace plugin
} // namespace stmros
#include <memory>
#include <thread>
#include <mutex>
#include <atomic>
#include <unordered_map>

namespace stmros {

// 插件基类（已移动到plugin.hpp）

// STMROS核心类
class StmRos : public std::enable_shared_from_this<StmRos> {
public:
  using SharedPtr = std::shared_ptr<StmRos>;
  
  // 构造函数
  StmRos(rclcpp::Node::SharedPtr node);
  
  // 析构函数
  ~StmRos();
  
  // 初始化
  bool initialize();
  
  // 关闭连接
  void shutdown();
  
  // 添加插件
  void add_plugin(plugin::PluginPtr plugin);
  
  // 发送消息到STM32
  bool send_message(const stmconn::StmLinkMessage & msg);
  
private:
  // ROS节点
  rclcpp::Node::SharedPtr node_;
  
  // 通信接口
  std::shared_ptr<stmconn::StmConnInterface> conn_;
  
  // 插件列表
  std::vector<plugin::PluginPtr> plugins_;
  
  // 消息处理器映射表
  std::unordered_map<uint8_t, std::vector<stmros::plugin::HandlerCb>> message_handlers_;
  
  // 通信参数
  std::string fcu_url_;
  uint8_t system_id_;
  uint8_t component_id_;
  
  // 状态
  std::atomic<bool> connected_;
  
  // ROS发布者
  rclcpp::Publisher<stmros_msgs::msg::StmHeartbeat>::SharedPtr heartbeat_pub_;
  
  // ROS服务
  rclcpp::Service<stmros_msgs::srv::StmCommand>::SharedPtr command_srv_;
  
  // 定时器
  rclcpp::TimerBase::SharedPtr heartbeat_timer_;
  
  // 回调函数
  void message_received(const stmconn::StmLinkMessage & msg);
  void connection_closed();
  void connection_error(stmconn::ErrorCode code);
  void send_heartbeat();
  void handle_command(const std::shared_ptr<stmros_msgs::srv::StmCommand::Request> request,
                      std::shared_ptr<stmros_msgs::srv::StmCommand::Response> response);
  
  // 加载插件
  void load_plugins();

  // 获取节点以供插件访问
  rclcpp::Node::SharedPtr get_node() { return node_; }
};

} // namespace stmros